/**
 *
 * History
 * modify rotateEarth
 * shoujian zhang, 2020
 * 
 */

#include "ComputeSatPos.hpp"
#include "YDSTime.hpp"
#include "constants.hpp"
#include "DataStructures.hpp"

using namespace std;
using namespace gpstk;

#define debug 0

namespace gnssbox
{


      // Return a string identifying this object.
    std::string ComputeSatPos::getClassName() const
    { return "ComputeSatPos"; }


      /* Explicit constructor, taking as input ephemeris to be used, 
       * default observable and whether TGD will be computed or not.
       *
       * @param dEphemeris    EphemerisStore object to be used by default.
       * @param dObservable   Observable type to be used by default.
       * @param applyTGD      Whether or not C1 observable will be
       *                      corrected from TGD effect or not.
       *
       */
    ComputeSatPos::ComputeSatPos( XvtStore<SatID>& ephStore,
                                    const TypeID& dObsOfGPS,
                                    const TypeID& dObsOfGAL,
                                    const TypeID& dObsOfBDS,
                                    const bool& applyTGDOfGPS,
                                    const bool& applyTGDOfGAL,
                                    const bool& applyTGDOfBDS )
    {
        pEphStore = &ephStore;
        defaultObsOfGPS = dObsOfGPS;
        defaultObsOfGAL = dObsOfGAL;
        defaultObsOfBDS = dObsOfBDS;
        useTGDOfGPS = applyTGDOfGPS;
        useTGDOfGAL = applyTGDOfGAL;
        useTGDOfBDS = applyTGDOfBDS;

    }  // End of 'ComputeSatPos::ComputeSatPos()'



      /* Return a satTypeValueMap object, adding the new data generated when
       * calling a modeling object.
       *
       * @param time      Epoch.
       * @param gData     Data object holding the data.
       */
    satTypeValueMap& ComputeSatPos::Process( const CommonTime& time,
                                              const SourceID& source,
                                              satTypeValueMap& gData )
            noexcept(false)
    {

        try
        {
            SatIDSet satRejectedSet;
            TypeID defaultObs;

            ///////////////
            // get satellite xyz
            ///////////////

            // Loop through all the satellites
            for(satTypeValueMap::iterator it = gData.begin();
                it != gData.end();
                ++it)
            {
                SatID sat( it->first );

                try
                {
                   Xvt svPosVel;

                      // compute satellite ephemeris at transmitting time
                      // Scalar to hold temporal value
                   double obs(0.0);
                   if(sat.system == SatelliteSystem::GPS)
                   {
                       defaultObs = defaultObsOfGPS;
                   }
                   else if(sat.system == SatelliteSystem::Galileo)
                   {
                       defaultObs = defaultObsOfGAL;
                   }
                   else if(sat.system == SatelliteSystem::BDS)
                   {
                       defaultObs = defaultObsOfBDS;
                   }

                   obs = (*it).second(defaultObs);

                   svPosVel = ComputeAtTransmitTime(time, obs, sat);

                   //
                   // transmitting-time related parameters
                   //

                   // relativity
                   double relativity(0.0);
                   relativity = svPosVel.computeRelativityCorrection()*C_MPS;

                   // clock bias, clock drift
                   double svclkbias(0.0), svclkdrift(0.0);

                   svclkbias  = svPosVel.clkbias*C_MPS;
                   svclkdrift = svPosVel.clkdrift*C_MPS;

                   // warning: the sign is changed!
                   // see that in LinearCombination.
                   (*it).second[TypeID::relativity] = -relativity;
                   // Let's insert satellite clock bias at transmit time
                   (*it).second[TypeID::cdtSat] = svclkbias;
                   (*it).second[TypeID::cdtSatDot] = svclkdrift;

                   // Let's insert satellite position at transmit time
                   (*it).second[TypeID::satXECF] = svPosVel.x[0];
                   (*it).second[TypeID::satYECF] = svPosVel.x[1];
                   (*it).second[TypeID::satZECF] = svPosVel.x[2];

                   // Let's insert satellite velocity at transmit time
                   (*it).second[TypeID::satVXECF] = svPosVel.v[0];
                   (*it).second[TypeID::satVYECF] = svPosVel.v[1];
                   (*it).second[TypeID::satVZECF] = svPosVel.v[2];

                }
                catch(InvalidRequest& e)
                {
                    // If some problem appears, then schedule this satellite
                    // for removal
                    satRejectedSet.insert( sat );
                    GPSTK_RETHROW(e);
  
                    continue;
                }


            } // End of loop for(satTypeValueMap = gData.begin()...

            // Remove satellites with missing data
            gData.removeSatID(satRejectedSet);

            return gData;

        }   // End of try...
        catch(Exception& u)
        {
            // Throw an exception if something unexpected happens
            ProcessingException e( getClassName() + ":" + u.what() );
            GPSTK_THROW(u);
        }

    }  // End of method 'ComputeSatPos::Process()'

    void ComputeSatPos::Process(gnssRinex& gData)
            noexcept(false)
    {
        try
        {
            Process( gData.header.epoch, gData.header.source,  gData.body );
        }
        catch(Exception& e)
        {
            GPSTK_THROW(e);
        }

    }  // End of method 'ComputeSatPos::Process()'


    Xvt ComputeSatPos::ComputeAtTransmitTime(const CommonTime& tr,
                                              const double& pr,
                                              const SatID& sat)
    {
        Xvt svPosVel;

        CommonTime tt;
        CommonTime transmit( tr );
        transmit -= pr/C_MPS;
        tt = transmit;

        for(int i=0; i<2; i++)
        {
            try
            {
                if(pEphStore==NULL)
                {
                    cerr << "pEphStore must be given" << endl;
                    exit(-1);
                }
                else
                {
                    svPosVel = pEphStore->getXvt(sat,tt);
                }
            }
            catch(InvalidRequest& e)
            {
                GPSTK_RETHROW(e);
            }

            tt = transmit;
            tt -= (svPosVel.clkbias + svPosVel.relcorr);
        }

        // transmitting time
        double dt = tt - tr;

        // earth rotation
        rotateEarth(dt, svPosVel);

        return svPosVel;

    };

    void ComputeSatPos::rotateEarth(double dt, Xvt& svPosVel)
    {

        double wt(0.0);
        wt = OMEGA_EARTH * dt;

        double sx(0.0), sy(0.0);

        sx = +std::cos(wt)*svPosVel.x[0] + std::sin(wt)*svPosVel.x[1];
        sy = -std::sin(wt)*svPosVel.x[0] + std::cos(wt)*svPosVel.x[1];

        svPosVel.x[0] = sx;
        svPosVel.x[1] = sy;

        double vx(0.0), vy(0.0);

        vx = +std::cos(wt)*svPosVel.v[0] + std::sin(wt)*svPosVel.v[1];
        vy = -std::sin(wt)*svPosVel.v[0] + std::cos(wt)*svPosVel.v[1];

        svPosVel.v[0] = vx;
        svPosVel.v[1] = vy;
    }


}  // End of namespace gnssbox
